#include "KinectInteraction.h"

KinectInteraction::KinectInteraction(){


}
using namespace cv;

/** @brief Iniatialize matrices and create processing objects
*   @param
*   @return
*
* Initializes matrices and processing objects.
*
*/
void KinectInteraction::init(){
    imgProc = new ImageProcessor();
    detector = new PeopleDetector();
    kinect = new KinectInterface();

    rgb = Mat(Size(640,480),CV_8UC3,Scalar(0));
    depth = Mat(Size(640,480),CV_16UC1);
    depthScaled = Mat(Size(640,480),CV_8UC1);
    mask = Mat(Size(640,480),CV_8UC1);
    gray = Mat(Size(640,480),CV_8UC1,Scalar(0));

    namedWindow("rgb",CV_WINDOW_AUTOSIZE);
    namedWindow("depth",CV_WINDOW_AUTOSIZE);

    running = false;

}

/** @brief Retrieve Kinect image and process to find people
*   @param
*   @return
*
* Start retrieval of Kinect images, use depth image as mask
* and detect people in foreground.
*
*/
void KinectInteraction::run(){

    //start retrieving data
    if(!running){
        kinect->run();
        running = true;
    }
    //get frames
    kinect->getRgbFrame(rgb);
    kinect->getDepthFrame(depth);
    depth.convertTo(depthScaled, CV_8UC1, 255.0/2048.0);

    //pre-process data
    imgProc->createMaskFromDepth(mask, depthScaled, 92, 122);
    imgProc->getChannels(rgb, channels);
    imgProc->getGrayscaleImg(channels, gray);
    imgProc->applyMask(mask, gray);

    //find pepole
    foundPeople = detector->findPeople(gray, mask, depthScaled);
}

/** @brief Prints people to XML-formatted string
*   @param
*   @return std::string containing people information
*
* Creates XML document and prints found people as elements.
*
*/
std::string KinectInteraction::print(){
    std::string result = "";

    TiXmlDocument doc;
    TiXmlDeclaration * decl = new TiXmlDeclaration( "1.0", "", "" );
    doc.LinkEndChild(decl);

    TiXmlElement *root = new TiXmlElement("People");

    doc.LinkEndChild(root);

    if(running){

        for(int i=0; i<foundPeople.size();i++){
            foundPeople.at(i).printXml(*root);
        }

    }
   //std::cout << result << std::endl;
    TiXmlPrinter printer;
    doc.Accept(&printer);
    result = printer.CStr();
    return result;

}

void KinectInteraction::draw(){
    if(running){
        imshow("rgb", gray);
        imshow("depth", mask);
    }
}

void KinectInteraction::stop(){
    cvDestroyWindow("rgb");
    cvDestroyWindow("depth");
    running = false;
    kinect->stop();
}
